package de.koller.aframe.util.search;

import de.koller.aframe.util.ALoop;
import java.util.ArrayList;
import java.util.HashSet;
import java.util.List;
import java.util.Set;
import java.util.TreeMap;
import java.util.TreeSet;

public class ATiledPathFinder {

	private final IATileMap map;
	private final ATile start, destination;
	
	private final TreeSet<ATile> openlist = new TreeSet<>();
	private final Set<ATile> closedlist = new HashSet<>();
	
	private final TreeMap< Integer, TreeMap< Integer, ATile > > buffer = new TreeMap<>();
	
	public boolean allowDiagonal = true;
	private double minH = Double.MAX_VALUE;
	
	public static ATile[] find( int xs, int ys, int xd, int yd, IATileMap map, boolean diagonal ) {
		ATiledPathFinder pf = new ATiledPathFinder( xs, ys, xd, yd, map );
		pf.allowDiagonal = diagonal;
		return pf.search();
	}
	
	private ATiledPathFinder( int xs, int ys, int xd, int yd, IATileMap map ) {
		this.start = getTile( xs, ys );
		this.destination = getTile( xd, yd );
		this.map = map;
	}

	public final ATile getTile( int x, int y ) {
		TreeMap< Integer, ATile > xr = buffer.get( x );
		if( xr == null ) {
			ATile t = new ATile( x, y );
			xr = new TreeMap<>();
			xr.put( y, t );
			buffer.put( x, xr );
			return t;
		}
		
		ATile t = xr.get( y );
		if( t == null ) {
			t = new ATile( x, y );
			xr.put( y, t );
			return t;
		}
		
		return t;
	}
	
	public ATile[] search() {
		openlist.add( start );
		
		int nl = 10000;
		do {
			nl--;
			
			ATile current = openlist.pollFirst();
			
			if( nl < 0 ) {
				System.out.println( "street min h: " + minH + " (" + ALoop.getMemory() + "MB)" );
				nl = 10000;
			}
			
			if( current.equals( destination ) )
				return makePath();
			expandNode( current );
			
			closedlist.add( current );
		} while( !openlist.isEmpty() );
		return null;
	}
	
	private ATile[] makePath() {
		List<ATile> reversed = new ArrayList<>();
		ATile c = destination;
		while( c != null ) {
			reversed.add( c );
			c = c.pre;
		}
		
		ATile[] path = new ATile[ reversed.size() ];
		for( int i = 0; i < path.length; i++ )
			path[i] = reversed.get( path.length - i - 1 );
		return path;
	}
	
	private void expandNode( ATile current ) {
		Set<ATile> successors = new HashSet<>();
		for( int x = -1; x <= 1; x++ )
			for( int y = -1; y <= 1; y++ ) {
				if( x == 0 && y == 0 )
					continue;
				if( !allowDiagonal && !( x == 0 || y == 0 ) )
					continue;
				
				ATile a = getTile( current.posX + x, current.posY + y );
				successors.add( a );
			}

		for( ATile successor : successors ) {
			if( closedlist.contains( successor ) )
				continue;
			if( !map.isWayPossible( current, successor ) )
				continue;
			
			double tentativeG = current.g + map.getWeight( current, successor );
			if( openlist.contains( successor ) && tentativeG >= successor.g )
				continue;
			
			successor.pre = current;
			successor.g = tentativeG;
				
			double h = map.getHeuristic( successor, destination );
			minH = Math.min( minH, h );
			double f = tentativeG + h;
			if( openlist.contains( successor ) ) {
				openlist.remove( successor );
				successor.f = f;
				openlist.add( successor );
			} else {
				successor.f = f;
				openlist.add( successor );
			}
		}
	}
	
}
